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ABSTRACT 


A Sliding mode compensator for depth control of an autonomous underwater 
vehicle (AUV) using depth feedback only is designed. The controller is evaluated 
for a nominal linear model and optimized by a Series of numerical experiments 
for a number of depth changing maneuvers. A state observer is used in order to 
estimate the unmeasurable states together with the sliding mode controller. The 
effects of varying control parameters are discussed. Compensator performance 
is assessed by numerical simulation of AUV dynamic response based on the full 
six degrees of freedom nonlinear equations of motion. The expected robustness 
of the design is demonstrated by comparison between linear and nonlinear vehicle 
response characteristics, and by a wide variation in vehicle parameters and 
hydrodynamic coefficients. Finally, suggestions for design improvement and di- 


rections for future research are indicated. 
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1. INTRODUCTION 


A. GENERAL 

There has been an increased interest recently in the need for autonomous 
underwater vehicles (AUV) in both Navy and private industry. A variety of un- 
classified missions includes ASW, decoy, survey, reconnaissance, and ocean engi- 
neering work service. As the cost of manned submarine vehicles increases, there 
are significant advantages to the use of cheaper unmanned vehicles. The AUV 
should be able to maneuver freely in the ocean enviroment with respect to depth, 
heading, and speed in order to carry out its missions. Such maneuvering require- 
ments have to be easily accomplished bv a low level active control svstem, and in 
the presence of environmental and phvsical uncertainty. 
All information concerning the environment of a vehicle 1s detected by the sens- 
ing level of control on-board the vehicle and directed to the high level intelligent 
system in order to carry out an unmanned mission. The dynamics of underwater 
Vehicles are described bv highly nonlinear systems with uncertain coefficients and 
disturbances that are difficult to measure. Robust control using variable structure 
systems are reputed to provide accurate control of nonlinear systems despite un- 
modeled system dynamics and disturbances, leading to the motion that sliding 
mode compensators should be employed in situations where accurate tracking 1s 
desired and where maneuvering parameters of AUV change with operating con- 


ditions. 


B. AIM OF THIS STUDY 

This thesis aims at investigation of the use of sliding mode compensator for 
AUV depth keeping and changing. The control concept developed here is that 
of a variable structure svstem consisting of continuous subsvstems together with 
suitable switching logic. The sliding mode control concept was suggested bv V. 
Baom Re land recently developed by J,J.E. Sloune [Ref. 2]. Because sliding 
mode control requires full state feedback. this work has incorporated a state ob- 


served based on output measurement resulting in a sliding mode compensator. 


The main goal of this thesis is to present a design procedure and сепи 
robustness of the variable structure compensator in the presence of vehicle non- 


linearities, modeling errors. uncertainties, and variation of parameters. 


C. THESIS OUTLINE 

Chapter 2 introduces the basic concept of Liapunov stability and an 
asvmptotically stable condition which is related to energy degeneration with in- 
creasing time for a dynamics svstem. The other sections will discuss how to de- 
sign a sliding surface and a control law based on a linear model. The last section 
of the chapter presents a technique to eliminate chattering in order lo PIO 
smooth control inputs. 

In Chapter 3, vehicle dynamics and a process used to produce я linear е 
space representation are described. Sliding control law for a lincar model is de- 
signed using results of the previous chapter. The design is evaluated through 
computer simulation. 

Chapter 4 presents the sliding mode compensator using depth measurements 
only. A model with perturbed hydrodynamic and geometric parameters is used 
to estimate the performance of the sliding mode compensator. The chapter ends 
with a discussion of the robustness of the sliding mode compensator and advan- 
tage of the variable structure control svstem 

Finally, Chapter 5 contains a summary. conclusions. and some directions for 


further research. 


to 


П. THEORETICAL BACKGROUND OF SLIDING MODE CONTROL 


A. GENERAL 

The dynamics of underwater vehicles are described by highly nonlinear, high 
order systems with uncertain models and disturbances that are difficult to model. 
A new form of sliding mode (Variable Structure System) control has been devel- 
oped recently, and shown to apply to a large class of nonlinear systems [Ref. 3]. 
Sliding mode control offers the control designer new possibilities for improving 
the quality of the control in comparison with a fixed structure system. The basic 
idea is to design a controller structure which consists of a set of continuous sub- 
systems together with suitable switching logic according to [Ref. 1]. The basic 


Sliding mode control for a SISO system 


x = Ах+ Ви 
w= ft Vx] (2.1) 
where. x IS the state variable 


u is the shding mode control law 
Y is a switched feedback gain 


AaB ате да ето EQUES 


This chapter is devoted to the study of the basic background of the sliding 


mode theory for the design of a linear controller for the AUV. 


В. LIAPUNOV STABILITY 
For a given control system. stability is usually the most important thing to 
be determined. [f the svstem is linear and time invariant, then. many stability 


criteria are available. such as the Nyquist stability criterion. the Routh’s stability 


criterion ete. The second method of Liapunov is the most general method for the 
determination of the stability of nonlinear and time varving systems. Before dis- 
cussing the sliding mode control, the second method of Liapunov will be dis- 
cussed in order to understand the sliding eondition, which will be discussed in the 
next section. The basic coneept of the second method of Liapunov js that if the 
svstem has an asymptotically stable equilibrium state, the stored energy of system 
decays with increasing time until it finally assumes it’s minimum value. In order 
to explain this, Liapunov introduced the so ealled Liapunov funetion, an imagi- 
nary energy function whieh depends on the state variable (x,,x,...x,) and time (t). 
If the Liapunov function is denoted by V(x,t) and it’s time derivatives denoted 
by 


з г 
Ао Бы (272) 


then. the Liapunov function has information as to stability, asvmptotie stability 
or instabilitv of an equilibrium state of the svstem without solving the state 
equation. The theorem of the Liapunov function is deseribed in modern control 


engineering [Re 4l Паши ов 


х = Дхы) (2.3) 


where xis the state variable and if 


(Ол) < 0, f, « t. 


and there exists a Liapunov function V(x,t) having continuous Mr and satis- 


fving the following conditions: 


I. V(x.) is positive definitey ana 


2. 1 (а г)ле перашау Че in 


then the equilibrium state at the origin is asymptotically stable. I’(x,r) is 
negative definite which shows that V(x,t) is continually decreasing. So for any 


system 


a uut) (2.4) 


where, x is the state variable 
u(t) is the control law 
x(0,t) = Xp 


and the time derivative of the Liapunov function F(x,u(t)) is negative definite, 


then control law u(t) is guaranteed stable. 


C. DYNAMICS OF SYSTEM WITH SWITCHING 
Now consider the case that an asymptotically stable system may consist of 
two Structures neither of which is asymptotically stable. If the differential 


equations of the second order svstem have the following format: 








dxi 
= X 
dt : 
E а (2.5) 
dt 


^ 


where ПОО с Ер 





Micwetie Structure Of the svstem is elliptic as described in Figure 1 on page 6. 
Suppose the system with a positive feedback gain, then structure is aperiodically 
unstable as shown in Figure 1. The block diagram of the closed loop system with 
switching gain is illustrated in Figure 2 on page 7. Let us trv to combine the 


advantage of both svstems by suitable choice of their structures in the appropri- 


сл 





Figure l. Asymptotically unstable structure [Ref.5] 


ate parts of the phase plane. In order to get the asymptotically stable structures 


in Figure 3 (b), the phase plane was divided into four pairwise subsections as 


shown in Figure 3 (a) by the following conditions: 


{2 


I uU 


Subsections |? 200 ТЕ! 
Subsection “Ws 2000 c a) 
Subsection ІІІ: хә т-ға! 


Subsections JV ua ss 


The good phase trajectory for each phase portrait has been chosen to make 


asymptotically stable structures as in Figure 3 (b). 


This phase plane ts separated from one another by the Straight line x, =O and 


х+ Ра #0 which we call the switch line or sliding surface. The asymptote 





Figure 2. The block diagram with switching gain 


x,+'’x,=0 acts as a switching line for the structure when the trajectory of the 
subsection I is reached. The structure of the system can be switched by using this 
line instantaneously from elliptic to hyperbolic in this case. This switching line is 
very important in sliding mode control. Once the system state trajectory ap- 
proaches the switching linc, and in order to keep the trajectory on the sliding 
surface for t >t, then this system will become asymptotically stable as long as it 
satisfies the Liapunov condition. In the general case, a switching line might be a 
straight line, ç = х,+4х, (0 < ¿ < ce), but must pass through the origin (ге 
с(0) - 0). 


D. SLIDING CONDITION 

Some of the possible advantages offered by the idea of switching the structure 
of a control system were described in the last subsection. But we remarked that 
H cucire does not change at the precise instance when the trajectory crosses 


the switching line, due to the effect noise, then additional control action will be 





Figure 3. Phase plane division and combined stable structure [Ref.5] 


required to enforce the sliding condition. The motion of the system now depends 
on sliding surface parameters which are insensitive to the external disturbances 
and variations of the plant parameters within a wide range of the switching line. 
If the system trajectory of the subsection I crosses into the switching line and af- 
ter passing the system trajectory of the subsection I] crosses over the switching 
line repeatedly again, the system trajectory will be kept within some range of the 
switching line. Suppose such change occurs at infinitely high frequency, then the 
state of the system trajectory is maintained with an infinitesimal amplitude oscil- 
lation. It is an asymptotically stable system on this line as shown in Figure 4 on 
page: 

The motion of the system on the switching line is described by the solution 


of the peneral differential equation (2.5) (Ref. 2] together with the equation of time 


f 


general swell sso 





Figure 4. Switching line and asymptotic trajectory [Ref.4] 


ЕЕ (< PEE een (2.6) 


where x =x—x, a single clement of the state vector 
д > 0 ( arbitrary constant) 


n= order of system 


lu motion delimed by the equation (2.6) describes the system dynamics in the 
sliding mode. A sliding mode has an important property that the corresponding 
motion of the svstem depends on the sliding surface (Switching Line) which is 


chosen bv the designer onlv. In order to know the mathematical existence condi- 


tion for the sliding mode, the theory of sliding mode has given a considerable at- 
tention to the methods guaranteeing the existence of the sliding mode. 
1. Condition for Existence of a Sliding Mode 


Considering the general dvnamic system 


E — f(x,t) (2.7) 


Let us assume that the right hand members of this equation are discontinuous 
on a certain sliding surface o(x) = 0 in the phase space, where, as phase trajectory 
of the svstem approaches o(x) =0 from either side, the following limits are de- 
ипей чери | 


Шт Лос) = уй) 


lim. fix.t) 2 f^ (x.t) (2.8) 
c— 0 
ЕТІ s 
then the derivative of the sliding surface (c) along the trajectories of the system 


15 


de _ to dx ера 
roo -I= (f.gradc) (219) 


where. {| = phase .elocity * ecto. and 


im س‎ rado 
с— Ü : -Us a ) 


10 


lim 40. = (ft grado) (2.10) 
Re, OG 


с-+ 0 
where o,f is a smooth function. 


At each point of ¢ = 0, the sign of the limit equation (2.8) has seven cases. There 
is a case Which is most interesting as it corresponds to the existence of an ideal 


Sliding mode on the sliding surface o = 0, if 


do 
— < 0 
one dt P 


: ас 
| — > 0 2.11 
Es at ( ) 


The equivalent inequality being the condition for existence of a sliding mode 15 


de ¢ 





lim O 
с-»0 at 


ОГ 





2. Proof of Stability 
This inequality is also suggested by [Ref. 5] as a necessary condition for 
the svstem in equation (2.5) to have a degenerating Liapunov function in the 


following form: 


Г(х) [c COT (2.13) 


|| 
| ںا 


11 


The asymptotic stability of the system (2.5) is guaranteed provided that I(x) is 
negative definite as discussed in the previous section. If c(x.t) is a sliding surface 


bv the definition above. it follows that 


a E > 
> G Cu) lo | (2.14) 


where i; is the sliding control gain and 


c(x,t) is the sliding surface. 


It will guarantee stabilitv of the sliding mode motion. The control law driving 
motion in the sliding mode can be obtained bv using this condition [Ref. 2]. The 
next section will be devoted to a discussion for the development of the sliding 


ПЛ СОП ТО ах 


E. SLIDING SURFACE DESIGN 

The sliding surface has a very important property that is shown in the previ- 
ous section. [he sliding surface should be designed so that svstem respon wa 
stricted to o(x) has a desired behavior. such as asymptoticallv stable state or 
tracking error. 


Let us consider a linear time invariant system to design the sliding surface 


х= Ах-+ Ви (22153) 
КНР E 


Consider that the sliding surface of the equation (2.15) has the following form: 


c(x) == E == () (2319 


where Sis thesitime surfaceeocmicien tt. (7777) 


The existence of the sliding mode implies that o(x)e(x) <0 and c(x) = 0 for all 


t 2 fy. Using the method of equivalent control [Ref. 6] 


OE 574 =0 (2.17) 


Substituting equation (2.15) for x of the above equation 
ҮЛЕС 
S (Axt Bus) + 0 


Or 


ео p. (2.18) 


Substituting equation (2.18) to Eq (2.15) and rearranging 
X= ANS BIS’ BIS’ Ax 
or 
х= [4=8(5: В) 5 Ах (2.19) 
Eemation (2.19) gives the dynamics of the system on the sliding surface for t > 4 


omen o(x) = 0, but the S matrix is unknown. In order to determine S matrix, the 


equation (2.19) can be rearranged in the following form: 
х+ | А4-ВА |х 
= (2.20) 


Nurs fessi SEE 
А = A— BK, 


The A, matrix can be obtained from the pole placement for which we can select 


Specifically desired closed-loop poles of the system equation (2.20) on the sliding 


surface. If we get the A, matrix by using the standard pole placement method, the 


sliding surface matrix (m x n) can be determined in the following procedure: 
К. = (5' В) '5'А 
$'А—5!ВК„=0 


5 (4-ВКД)-54,-0 (2.21) 


It should be noted that A, must be rank deficient by one and that the procedure 
must therefore place one pole of A, at the origin. The left eigenvector of the A, 
matrix of equation (2.21) corresponding to a pole placed at the origin are the 
Sliding surface coefficients which give the desired behavior system on the sliding 


Slim ace 


Е. SLIDING MODE CONTROL LAW 

Given the dynamic model. the sliding surface definition, and the stability 
criteria. a Suitable control] jaw can be obtained. We assume that a wide range of 
single input, single output dynamic systems, and sliding surfaces can be described 
jo 


- 


х= 4х+Ви (2.22) 


с(хы)- 81544) 


where S! is a row vector of the form [1], 5), 5,....5,_,]. a Specilic choice om NT 
achieve a stable tracking error and to enhance robustness as discussed in the 
previous section. If w(t) could be chosen so as to keep the trajectom mee 
c(x.f) 20, we would have the sliding control law from the slidine condam 


equation (2.14) and sliding surface equation (2.16) that 


o e= 515 = —n*sign(c) 


S'A+S' Bus —nosign(c) 
u = —(S'!By |S' Ax-(S' BY nesign(o) (2.23) 


where mà isa arbitrary nonlinear feedback gain 


The control law has two parts. 
u = +u 


where и 15 linear feedback control law 


uw is nonlinear feedback control law 


Initially. « compensates directly for the known portions of the dynamics. Thus. 
И 15 discontinuous across the sliding surface. This nonlinear term is obtained di- 
rectly from the time-varving bounds on parametric uncertainty and disturbances. 
As a result, control discontinuitv across c-Ü grows as the model becomes less 
certain and increasinglv disturbed. This insures that e^ is a Liapunov function 
of the closed-loop svstem. since it satisfies the sliding condition equation (2.14) 
and thus guarantees stability despite the uncertainty in the model and disturb- 
НЕЕ |Кег 21 This type of control law can guarantee stability and perfect 
tracking for a large class of nonlinear systems. The discontinuous form results in 
a chattering tvpe of control action that would be very undesirable for most svs- 
tems and this chattering behavior has been one of the main reasons sliding con- 
trol techniques have not been more widelv applied. This problem will be solved 
by smoothing out the control law in a thin boundary layer around the sliding 


n ice ds слей іп the next section. 


С. CONSIDERING CHATTERING AND UNCERTAINTY 

While sliding mode control provides a control law which is robust to param- 
eter variations and disturbance inputs, it was a chattering problem for the input 
as shown in Figure 5 on page 17. In fact, imperfections such as delays in 
switching and hvsteresis in switching, will cause the trajectory to chatter along the 
sliding surface. Although as such imperfections vanish, control activity remains 
as undesirable switching and high frequency signals on the sliding surface. 

The basic idea of eliminating chattering is simple. This chattering problem 
caused by a discontinuous and nonlinear switching feedback control law can be 
eliminated by replacing it with a continuous feedback control law. But if the 
Sliding mode control has a continuous feedback function, there are steady State 
errors due to variations in parameter and disturbance [Ref. 2]. Suppose the con- 
trol law has a continuous feedback, whose terms are continuous function inside 
a small boundary laver thickness on the sliding surface, as shown in Figure 6 on 
page 18, then the steadv state error can be calculated by a smooth function which 
eliminates chattering in the boundary layer thickness (¢). This boundary layer 
thickness can be determined directly from the desired sliding surface coefficient 
limit and estimates of the uncertainty dynamics of the system to be controller. If 
the specified bounds on disturbances and parameter uncertainty are not ex- 
ceeded. the system is guaranteed to stay within the boundary laver once inside. 
lf a disturbance temporarily exceeds the specified bounds, the state may go out- 
side the boundary layer. However, the sliding condition equation(2.14) implies 
that the svstem will alwavs move back inside the boundary laver once the dis- 
tupbanices return tell projected l v l 

The dynamics of the state trajectory inside the boundary are only an ap- 
proximation to the desired dynamics on the sliding surface. The advantage of the 
scheme is that the state trajectory does not chatter close to the sliding Surface: 
To carry out the preceding program, we use the sliding surface considered in the 


previous section with o(x.¢) of the form: 


clx) = 4,x(0) (2.24) 
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Iigure 5.  Chattering problem 


x(t) 


Where 
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= x(t)—xA0) 
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To define the boundary layer thickness about the sliding surface of equation 


INED define 


where 


It is immediately from equation (2. 


o is the boundary layer thickness 


Usted 


nO 


с(н)- 


а) boundary layer thickness b) saturation function 





Figure 6. Boundary laver thickness and saturation function 


[о(х) | = $ (2.26) 
апа 
TE a OE 12 
о (x di Elx) = ye (xen) (2.27) 


We choose the control law u(r) as by equation (2.27) for о (хх < OR 


G^ (x,t) 7 0. This guarantees that 


— c (x,t) > 0 (2.28) 
dt 
= Cl ©0 (2.29) 


Equation (2.28) and (2.29) establish that trajectories Starting outside boundary 
layer tend towards boundary layer, and further trajectories starting inside 
Boundary layer stay in it for 7, < t. [t only remains to specify u(x,t) to be a con- 


tinuous function of x inside boundary layer thickness. 
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ІП. SLIDING MODE CONTROL FOR NONLINEAR A.U.V. IN DIVE 
PLANE 


A. GENERAL 

Underwater vehicles present difficult control system design problems due to 
their nonlinear dynamics, uncertain hydrodynamic coefficients and the presence 
of disturbances that are difficult to measure or estimate. This chapter describes 
the dynamics of a selected automonous underwater vehicle (AUV) and the design 
of a sliding mode controller which can handle these problems effectively. 

Motions of underwater vehicles are expressed in a body fixed reference frame, 
because hydrodynamic forces and inertia properties are most readily computed 
in a ship reference frame. The nonlinear equations of motion in six degrees of 
freedom vehicles which are commonly known as the DINSRDC 2510 equations 
of motion are used for verification of the sliding mode contro! design. These 
highly nonlinear equations of motion are linearized by a Taylor series expansion 
and modified to suit the needs of an AUV [KRef. 7]. First, these hnearizedmml 
modified equations of the svstem are used to design the sliding surface. Sliding 
mode control law and observer. These values of the linear system are, thema sS 
to implement the sliding mode control law for the nonlinear svstem. This chapter 


shows how to design the sliding mode control for the highly nonlinear AUV. 


D. NONLINEAR COMPUTER MODEL 

The nonlinear model used for sliding mode control verification was derived 
from the original NSRDC 2510 document [Ref. 8]. The nonlinear model used in 
this thesis consists of 8 differential equations which describe the AUV dynamics. 
The six equations as derived from force and moment equalities account for the 
states u. v. w. p. q and r. The shape of AUV. which 15 17.4 feet long. wera 


12000 pounds, and neutrally buovant. is depicted in Figure 7 on page 21. 
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Fieure 7. Sketch of the autonomous underwater vehicle 


General dynamical equations are derived from Newton’s law in an inertial 


reference frame: 


—» 


Ғ 
| il = f( dynamical response terms ) (3.1) 


The general form of the force balance is 





pe £ (Mx) (3.2) 
“nerc M = mass matrix 
р 
and the moment balance is 
M=— (19) (3.3) 


where I = moment of inertia matrix 
OQ < | р.д.г 1" 


Three dimensional motions of underwater vehicles are normally described using 
the body-fixed coordinate and inertial reference frame. Position of the body-fixed 
coordinate system is expressed in X, Y, and Z coordinates and orientation of the 
vehicle's coordinate system in expressed in Euler angles Q. 0, and vy. The defi- 


nitions of U, v. w, p. q. r and controls are listed in Table 1 and Table 2. 


Table 1. DEFINITION OF A U Y STATES. 


— —8 | [шз 3 













Table 2. DEFINITION OF A.U.V CONTROLS 


CONTROL DEFINITION 
eee --- 


starboard bow _. angle 


port bow plane angle 


Stern TM angle 











The dynamical response terms of the left hand side of equation (3.1) or 
Emmon (52) and (3.5) express the external forces and moments exerted on the 
vehicle bv hydrodynamic. control surface, propulsion and other effects. The force 
and moment equalities of equation (3.2) and (3.3) describe motions in six degrees 
of freedom of the AUV. The three forces are in the axial, lateral and normal di- 
rections which give rise to motions in surge, sway and heave respectively. The 
three moment equations produce moments and motions in roll, pitch and yaw. 
Figure 8 on page 24 shows the positive directions of forces. moments. motions, 


and control surface dellections. 





Figure 8. Positive motion directions of the AUV 


The equations of motion for the six degrees of freedom for the fully nonlinear 
model are listed in the following page. The hydrodynamics coefficient of those 
equation used for this thesis are those that were determind using an analytic ap- 
proach [Ref. 7] and later simplified [Ref. 9]. The four nonlinear equations that 
are considered for designing sliding mode control of the AUV are written in the 


following form: 


Normal Equation of Motion 


тПу-и д+тр+А Дрг--д)+ Vr p)- Zyp^-q^)] = 
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p "E / Е Е m 
EE [Z 44-2 рр? +2 рг+ 2 ا‎ 


F 5- PLZ WZ! gl G+Z' ypXPtZ'y VT] 


BN , / 2 2( 7! Са 
+ = Е (г ког вьдь) 


(w—xq) 


IR dx 
7% 


— [ec p,h(x)(v--xr)^ - Cp,b(x)(w—xq)^] 


T(IW— B)cos 0 cos ф 


2 


(2 w +Z' “26 )e(n) (3.4) 


wy x 057] 


DNE. i 
xs p Z GENIT >! 


Pitch Equation of Motion 


E TDP TDP FLAP =F j-m[A,(w—u,q+vp)—Z,(u—vr+w9q) | = 


PEM prprtM rrr?) 


P ,5 ; / 
[M qd M ppl 


) 
Td = PEM Ме Ма Мар М, ле) 


+ a PLA tt WAL уи (А, бр”, б 


р ғ c к” ) (w—xq) E 
– —- en o | ЕЕ хах 
p c 


a 


—(X,IV—X5B) cos 0 cos 96—(Z,W—Z 5B) sin 0 


) 
a i ГМ ше + = POM pM WM i U5 EC) В 


Kinematic Relations 


0 = q cos ó—r sin ó (3.6) 


Z — —u, sin Ó--v cos 0 sin $+w' cos 0 cos ф (3.7) 


Ihe simulations for the dive plane control were performed by using the 
FORTRAN language code (Appendix. B) for the simulation of nonlinear system 


response as a function of time. 


C. LINEAR MODELING 
]. Equations of Motion 
The sliding mode controller design procedure begins with the expression 
of the equations of motion in linear time invariant state space form. Te 


nonlinear AUV svstem is 


-&. x(r) - M^ fix(t), u(t)) (3.8) 


dt 
D (3.9) 


NV dene Х the state .eelen 
Ш 15 ae tee Penal ngo UL CIE OT 


Е ало асот 


Although equations (3.8) can be significantly simplified as in Larsen [Ref. 9]. they 
appear still verv complex for this studv. The nonlinear equations can be linearized 


through a Taylor series expansion in the vicinity of a nominal point (ideally where 


< ШЕЕ О tor small deviations of u(t) and x(t)sfrom the reference values. These 


equations of motion were linearized by Boncal [Ref. 7]. Alternatively. least 
Squares techniques can be used for parameter identification in order to develop 
a linear model for the relationship between dive plane angle (0). pitch rate (q), 
depth (z) and pitch angle (0) [Ref. 10]. The linearized and very simplified 


equations for the dive plane motion 


g = —0.7q—0.030—0.0356 
0-4 
¿= —UÜ9 (3.10) 


were found to provide a satisfactory approximation of the open loop dive plane 
dynamics of nonlinear equation (3.4) to (3.7) for the nominal speed of 6 ft/sec (or 
500 rpm ). 
2. Sliding Surface Design 

It is evident from the discussion of the sliding mode theory (Chapter 2) 
that description of AUV motion depends on the sliding surface regardless of dis- 
turbance and unmodel parameters, after it hit sliding surface. So it is very im- 
portant to design sliding surface of the AUV with disturbance and unmodelled 
parameters. The sliding surface of the AUV will be designed based on the linear 


model. The state space form of the linear model is 
x= Bu 
ad (Salt) 


where t] 


i) OE 
ru NI 0 0 
Оа 0 
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а= 


220-055 


p= 0 
0 
u = ò 
and 
c 00 


For a three dimensional system, the sliding surface is the Euclidean plane 
gl) = х= 


RCE 


53 
Equation (2.6) can be expressed in terms of the State variables and sliding surface 
coefficients as following 
(xX) - 4+50+ 2 ET 
where the coefficient of q has been normalized to 1. S will determine the sliding 
surface plane uniquely. To compute the equivalent control (14,). we substitute A, 


B and S in to equation (3.11). and the closed-loop dvnamics of the linear model 


ake 
x2 [A-B(S' By ° p 
ОГ 


x 2 (A-BR)x (3.13) 


where the gain vector K can be found from standard pole placement methods. 


The closed-loop dynamics matrix 
4А. = А-ВК (3.14) 


ее АВ 


has eigenvalues specified for desirable response of the AUV. One of the 
eigenvalues of A. must be specified as zero. With A, specified and K computed 
from pole placement, S’ matrix can be determined as using the equation (3.13) 
and (3.14). 


S"(4—BK)=0 


ОТ 


Therefore S" is found as a left annihilator of A, or S is a left eigenvector of A, 
which corresponds to the zero value. This sliding surface of the AUV satisfy the 
Sliding surface condition S’x =Q. To find proper sliding surface of the AUV, 
Matrix-x program “SCM” in Appendix A is used. The response of the AUV ac- 
ШЕШЕ 10 different sliding surface is shown in Figure 9 on page 30, Figure 10 
on page 31, and Figure 11 on page 32. The sliding surface of Figure 10 which 
has no overshot and fast response time is selected as the reference sliding surface 


of the AUV in order to design control law. The sliding surface of the AUV is 
с(х) = 9+0.520—0.01122 (3916) 


This sliding surface has the desired dynamics of the closed-loop system. The 
perfect depth tracking of the selected AUV is then defined as remaining along the 
surface. The dynamic response of AUV is affected by the chosen sliding surface. 


The selected sliding surface is used to handle accurate depth control. 
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Figure 9. The dynamic response for the closed-loop poles = 0. -0.1,-0.15 
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Неше 10. The dynamic response for the closed-loop poles = 0,-0.25.-0.27 
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Figure 11. The dynamic response for the closed-loop poles = 0,-0.45,-0.47 


3. Sliding Mode Control Law 
Here the goal is to determine switched feedback gains which will drive the 
AUV state trajectorv to the sliding surface and maintain sliding mode conditions. 


By defining the Liapunov function of the equation (2.13) 
] 2 
V(x) = = [a(x)} (3.17) 
where c(x) 2 q4-0.520—0.0112z 


asymptotic stability of the AUV on the sliding surface is guaranteed, that pro- 


vided (x) is a negative definite function or 


с(х)б(х) = —т | о(х)| 
or 
о] = —›°зїен(с(х)) (3.18) 
Е ©) = Six, we have 
$ (Ах+ Ви) = —n sign(c(x)) Eo 


and solving for the equivalent control input u 
и = —(S!By S! Ax-(S! By N (3.20) 


It is important to recognize that the feedback control law is composed of two 


parts, 
f. = 
u=u+u 
The first w= —(STB)- S7 4x is a linear feedback control law, where the second 
и = —(S'Bywsign(c(x)) is a nonlinear feedback with its sign toggling between 


plus and minus according to which side of the sliding plane the AUV is located 


in. Two comments are in order here: First, since и has to change its sign as the 


AUV crosses o(x) = 0, the sliding surface has to be a hyperplane (dimension of 


one less than the state variable). Second, it is # which is mainly responsible for 


driving and keeping the AUV onto the shding plane o(x) = 0 (where == 


well). The block diagram of the sliding mode control is illustrated in Figure 12 


on page 34. 
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Figure 12. The block diagram of the sliding mode control 


Qutput 





Provided that the gain has been chosen large enough, ; can PION 


required robustness due to momentary disturbance and unmodeled AUV without 


any compromise in stability. The linear feedback law is designed such that the 


AUV has the desired dynamics on the sliding plane. Since a(x) = 0 in this case 


M 


и=й=—(5'В) 5! Ах (3.21) 


Large enough gain (ETA =n?(S’7B)-!=4.0) could be used in the computer simu- 
lation by using Matrix-x and FORTRAN program “SMC” in Appendix A. The 
dynamic response of the AUV according to several values of gain is shown in 
Figure 13 on page 37, Figure 14 on page 38, and Figure 15 on page 39. Choos- 
ing properly large enough nonlinear feedback gain, although the response time 
of the AUV is fast, the AUV overshoots on the sliding surface as shown in 
Figure 15 on page 39. The dive plane angle of the AUV presents chattering due 
to sign switching of the nonlinear part of the control law. Several varieties of 
nonlinear feedback gain (17) and boundary laver (9) are evaluated through a se- 
ries of numerical experiments in order to analysia dynamic response of AUV 
which are dependant on it. The characteristic of dvnamic response according to 
the nonlinear feedback gain, boundarv laver, and closed-loop poles are shown in 
Table 3. 


Table 3. THE DYNAMIC CHARACTERISTIC OF THE AUV ACCORDING 
TO DESIGN PARAMETER s 


Parameters Depth re- Dive plane Pitch angle Sliding sur- 
sponse angle (radians) face 
slow, no no saturated maxi no overshoot 
overshoot angle 7 0.4 
fast, no over- | no saturated maxi no overshoot 
shoot angle — 0.41 


ЄР fast, over- saturated тах! overshoot 
0.-0.45.-0.47 shoot angle =0.42 

ETA 23, slow. over- no saturated maxi overshoot 
IM — 0. shoot angle = 0.4 

ЕТА =4, та по ОӨкет- | mno saturated max} no overshoot 

PHI — 0.4 shoot angle — 0.41 

Е - 2 fast. no over- saturated пах! overshoot 
shoot angle — 0.42 





We already knew that system behavior 1s dependent on the sliding surface which 
is designed based on closed-loop poles. The closed-loop poles (0,-0.25,-0.27), 
nonlinear feedback gain (ETA =4), and the boundary layer (@ =0.4) are selected 


in order to Satisfy the necessary motions of the AUV. 
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Figure 13. The dynamic response for ETA(3) and PEII(0.2) 
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Figure 1d. The dynamic response for ETA(4) and PHI(0.4) 
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Figure 15. The dynamic response for ЕГА(8) апа РНЦ(0.4) 
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4. Chattering Problem and Steady State Error 
The nonlinear part of the sliding mode control may give rise to chattering 


where constant or random disturbances are present, as shown in Figure 16. 
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Figure 16. The chattering problem of sliding mode control 


A choice of a saturation function (satsgn(o)) instead of the pure switch (sign(o)) 
for the nonlinear control law ts prefered in order to avoid chattering problems. 
Since the boundary layer thickness is related to the characteristic of dy maumea 
sponse, it can be selected according to the manuvering conditions of underwater 


vehicles. Therefore the control law for the AUV is modified to 


> 
55 
А 
“Эе 


и = —5.1429q-2-1.07140-- K,satsgn(a) ( 


where Ky = (STB)! 


and 
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+1 | озо 
satsgn(o) = —1 iff в<-ф 


F3 if -ф<а<ф 


This provides a linear interpolation across the boundary layer as illustrated in 
Figure 17 on page 42. 

A boundary layer thickness (@) essentially assigns a lowpass filter structure to the 
dynamics of the sliding surface (c) . If a specific bound on disturbance is not ex- 
ceeded, the system is guaranteed to stay within the boundary layer once inside. 
The steadv state error of the sliding mode control of the AUV with a disturbance 


can be computed as following. From the equation (3.11) 
q = —0.74—0.030—0.035u4-d (3.24) 


where d is disturbance 
ps 


0 = 0 in steady State 


The steady state control input for the given disturbance 15 





d Е 
и == - 3.25 
s 0.035 AE 
The contro] law in steady state 15 

Кг 2). 

u |. E = no satsen( £) (3.26) 

Q) 

where c(x) 2 —0.01 12e, 
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Figure 17. The saturation function for the nonlinear control law. 
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The steady state cerror can be minimized bv increasing the nonlinear contro meaai 
(17) and decreasing boundary laver ($) as shown in Figure 18 on page 44. How- 
ever, It can never be completely eliminated, The depth response of the T im i 
(a) show the steady state error for the constant disturbance (d =0.005) when us- 


Ing the nonlinear feedback gain (ETA =4). Large nonlinear feedback gain is used 


ШКО Ше to decrease the Steady state error as shown in Figure 18 (b). The steady 
State error for the command depth is decreased down to 10% approximately us- 
ing large gain. However, dive plane is kept the same angle regardless nonlinear 
feedback gain. If we use large gain and small boundary layer, the depth response 
of the AUV may overshoot and the dive plane input may have numerical chat- 
tering. This is different from the chattering due to disturbance and unmodeled 
dynamics as shown in Figure 19 on page 45. This numerical chattering problem 
is an artifact of the numerical integration method (a fixed step Kutta-Merson and 
fixed Euler's method were used in the simulation) and, in principle, it can be re- 


duced by choosing a more accurate method with smaller time step. 
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Figure 18. The steady state error for increasing nonlinear feedback gain 
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Figure 19. 
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5. Robustness tests 

Another important aspect of the sliding mode control is its robustness to 
parameter variations and unmodeled dynamics. Sliding mode controllers are ex- 
pected to outperform more conventional linear state feedback with respect to 
robustness. This is true for the following two reasons: First, choosing y? suffi- 
ciently large will guarantee that the system approaches the sliding surface 
asymptotically even for the off-design case. Second, since at the final stage the 
system evolves in a lower dimension state space (the sliding surface), it is na- 
turally more robust than the original higher dimensionality system. To verify 
these expectations, we changed the hydrodynamic coefficient of the AUV, AUV 
rpm and dive plane strength as in Table 4. The standard design AUV equation 


are designated by A, B matrices at 6 ft/sec (rpm = 500). 


Table 4. ROBUSTNESS TEST CASES 


[Копа | — A — | в [| 6100 | 
Tl | А [в [61 60) 
[tens | A3 — | — 5 — [hse G0) — 
[me | A2 [52 — [fee 89) | 
[А в [ЗА 0000 | 

















The sliding mode control for the AUV was designed based on the nominal case 
as in the previous section. The nominal nonlinear feedback gain (у? = 2.4) апа 
boundary laver thickness (@ = 0.4) were applied to the modified system. The dy- 
namic response of the modified AUV is shown in Figures 14, ап 20, 10 26. ТІС 
A matrix of test 1 model, which is related to the rotary damping coefficient, was 
doubled in magnitude in order to test the sliding mode control. Only the response 


time of test 1 model was longer. The test 3 model. which has a matrix equal to 
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one-half of the A matrix, presents fast response and overshoot due to decrease in 
the the rotary damping moment as shown in Figure 22. The hydrodynamic coef- 
ficient for the dive plane, which in proportional to dynamic response time, was 
changed in order to test performance of variable structure system for the AUV. 
In general, it is evident from the simulation results, that even under a 200% 
change in the coefficient of the A, B and RPM, the vehicle response remains 


stable, 7 ~ 8 % overshoot, faster or slower as expected. 
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Figure 20. Ihe response of 2*A, B and 500 rpm 
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Figure 21. 
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Figure 22. The response of A/2, B and 500 rpm 
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Figure 23. 
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Figure 24. The response of 2*A, 2*D and 500 rpm 
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Figure 25, The response of A/2, B/2 and 500 rpm 
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The response of A, B and 1000 rpm 


Ficure 20. 


D. DESIGN OF SLIDING MODE CONTROL FOR THE 
NONLINEAR MODEL 

The sliding mode appeal is based on its ability to treat nonlinear, time varying 
and unmodelled svstems in a straightforward manner [Ref. 3]. It is the purpose 
of this section to apply the linear sliding mode controller to the nonlinear model 
of the actual AUV. The sliding surface and control law gain for the actual AUV 
are based on the linearized nominal model. The mathematical form of the non- 


linear model for the actual AUV is 


5 
S =Ах+АДх)-+ B(x)ud- d(x, t) (3.29) 
G 
where B(x) is the nonlinear function associated with the control 
Surface and actuator svstem 
Afix) is the uncertainty of the nonlinear function f(x). 


a(x. t) 15 the uncertainty disturbance. 
The system matrix A 1s the estimate of f(x) and the magnitude of the uncertainty 


is bounded as 


Е> | $ АДХ) | (3.30) 


where Е = 


| В 
| | 
The individual bounds on any element of Af as estimated from some knowledge 
of the extremes of possibility of A/{x). Also. let B(x) be approximated bv B, a 
constant. where the varving gain f is defined by B= fi B(x), and f) is taken to be 


Eu bstbounded within the лошо Рик Вж Ра: and D, = 1. Then it 


"5 


follows that f 2 1, and Af 2 0, and d(x,t) *0 will yield the nominal sliding mode 
control law (3.20). Disturbance d(x.t) is unknown but is upper bounded by а 


known continuous function such that 


(3.31( ا د 


The dynamics of the system with bounded uncertainty of the actual AUV is 


< = Ax+Af(x)+f Bu+d(x, t) (3.32) 


This equations is used to simulate the uncertain nonlinear terms after the 
linearization process. We have designed the sliding surface o(x) = S’x by using 
the nominal linearized model in the previous section. Using the sliding condition 
theory for the actual AUV, the sliding mode control law for the nonlinear model 


can be chosen from equations (3.17) and (3.18) such that if 
: 2 
e(x)e(x) 2 —w | o(x) | 
and the system will reach the surface (a(x) = 0) within a finite time 2 аси 


pod Loi (3:33) 


- 


H] 
The true dynamics of the sliding surface with uncertainty are, however, given by 
6(х) = 5/5 
- 5 Гах+АДх) + Ви-Қх. 11 (3.34) 


Substituting equation (3.20) to the above equation for u, then the derivatives of 


Ox 
& -(1—-f ys Ax S! Aftxy- S! d(x, n 


- Ви мене) (3.31 


From the above. stability is guaranteed, if and only if 
G(x) < —nisign(s), (3.36) 


where 1s the nonlinear feedback gain without an uncertainty 


Pee control matrix B is exactly Known (f = 1), then control law 
u,— —[S B] |S' Ax-L[S! B] и нете(х)) (3.37) 
where n? > | ng+F(x)+D(x, 6) | 
сап guarantee stability and perfect tracking for the nonlinear svstem with con- 


stant control matrix of the AUV. In case where the control system B(x) is un- 


certain. the following change (y+) must be made: 
c= -Г87В1 15” Ах-Г87ВТ ің“ епс(х) (3.38) 


u = +1 
where u = —-[STB]-ST Ax 
и = —(S7BI-'n-sign(c) 
о 





ЕШ Пе попппеаг (егіз о? Іле В(х) апа A/{x) is uncertain in most cases, it is 
"ШОШО to be zero equation (3.29) and 77 is increased depending on their as- 
sumed bonuds to guarantee stable sliding mode control [Ref. 2]. The actual 
control law used in subsequent simulation was in fact equation (3.38) with suffi- 
Seni. aree y- to accomodate the uncertainty in. Af(x) and B(x). Ihe sliding 
Iude switching control law equation (3.38) guarantees that equation (3.18) is 
Satisfied even in the presence of parameter variations and unmodeled dynamics 
provided q^ is large enough. The dive plane angle chattering due to modeling er- 


rors and disturbances can be eliminated by defining a boundary layer thickness 


@ about o = 0 as illustrated in the previous subsection. The smooth sliding mode 


control law,ol the actual AU Vor еп 


И = -[57В1 (57 Ах+ ване P | (3.39) 


where 4° > Bax | mgt F(X)+ D(x, 2) | + | (Bmax—1) | | STA | 


The dynamic response, dive plane angle, and sliding surface obtained from non- 
linear model simulation at 500 rpm are shown in Figure 27 on page 59. A\l- 
though the sliding surface and control law gain based on the nominal linear 
svstem were applied to the nonlinear system, the response of the system is satis- 
factory as expected. The designed variable structure system based on the nominal 
linear equations dealt with the full nonlinear dynamics of the AUV as shown in 
Figure 27. The technique of sliding mode control can handle the nonlinear sys- 
tem directly without linearization. if sliding surface coefficients are properly cho- 
sen. This is especially important for highly maneuverable underwater velicles 


that can move in albdimection. 
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Е. ROBUSTNESS TESTS USING THE NONLINEAR MODEL 

The variable structure systems have an important property which is much less 
sensitive to nonlinearities, disturbances, and unmodeled dynamics as illustrated 
in the previous chapter. The nonlinear model of the AUV is used to demonstrate 
the properties of this control methodology. Accurate models are normally re- 
quired in order to achieve good control. However, since hydrodynamic modeling 
is a key element in the design of control system for AUV, we have to rely on ap- 
proximate expressions for the hydrodynamic forces which can introduce large er- 
rors into the control process. We have already seen the results of simulation for 
control performance using an accurate linear model, modified linear model and 
accurate nonlinear model. In this section, a simulation study is performed to il- 
lustrate the effectiveness of the sliding contro] under large modifications in the 
nonlinear model parameters. The modified hydrodynamic coefficients in Table 
4 are closely related to system stability. The rotary damping coefficient (A/,) а!- 
fects the hydrodynamic moment of the AUV in the vertical plane. The center of 
gravity center of buovancy separation is directly proportional to restoring mo- 
ment. Hydrodynamic coefficient (.\/,,.) directly affects the pitch moment gener- 


ated by the dive planes. Speed was changed from 500 rpm to 1000 rpm. 


Table 5. TEST CASES FOR THE NONLINEAR MODEL 





The controller used was based on the nominal linear model. while some vehicle 


coefficients differed bv 200^o and nonlinear terms were added. The PORTE 
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program in Appendix B was used for this purpose. Time plots of depth, pitch 
rate, Sliding surface, and pitch angle response along with the depth commanded 
values are shown from Figure 28 on page 62 to Figure 33 on page 67. 

Ihe sliding mode controller is shown to provide stable response and performs 
consistently at different parameter values and speed. In every case, the system 
remains inside the boundary laver (c — 0). It is felt that the sliding mode method 
can produce extremely robust controllers that perform predictably despite the use 
of simplified or unmodeled dynamics. The center of gravity of the AUV is the 
most Sensitive parameter in the vehicle dynamic response. The depth response 
of the AUV did not overshoot. although the center of gravity was close to the 
center of buovancy. In general, the rpm affected the dvnamic response of the 
nonlinear vehicle. In this case. although the rpm was doubled in magnitude, the 
overshoot was not present and the response time reduced to 35 seconds. When 
Variation of parameters and modeling error of the AUV were increased, the 
robustness of the sliding control was improved by increasing the nonlinear feed- 
back gain (47). Of course. if the uncertain values exceed the limited boundary, 
the sliding mode control will not handle these variations effectively. The chosen 
nonlinear feedback gain (i = 2.4) for the selected AUV was enough to handle the 
nonlinear terms and the modified hvdodvnamic coefficients. The modeling er- 
rors, variations of parameters, and nonlinear terms, which were difficult problems 
of robust using other control technique. were easilv dealt with using the sliding 


mode control as shown from the results of simulations. 
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[igure 28. The dynamic response of test 1 AUV (depth command = 100 ft) 
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Figure 29. The dynamic response of test 2 AUV (depth command — 100 ft) 
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Figure 31. The dynamic response of test 4 AUV (depth command = 100 ft) 
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The dynamic response of test 5 AUV (depth command — 100 ft) 
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Figure 33, The dynamic response of test 6 AUV (depth command — 1UU ft) 
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IV. DESIGN AND EVALUATION OF A SLIDING MODE 
COMPENSATOR 


A. GENERAL 

An autonomous underwater vehicle (AUV) must operate under its own power 
and be capable of navigation and guidance with sufficient accuracy to be easily 
recoverable. A navigation system for an underwater vehicle is subject to vehicle 
size Or cost restrictions and this limits the ability to install the highly accurate 
sensors needed to produce reliable pitch rate data. The vehicle depth can be 
measured directly by a pressure cell sensor aboard the vehicle. Using the depth 
of the vehicle as the only external input, state observers can provide all remaining 
controller data. It is desirable to investigate the performance of a sliding mode 
compensator designed for a linear model and applicable to a nonlinear model 
with unmodeled vehicle dynamics. It is the purpose of this chapter to design 
such a sliding mode compensator and assess its robustness. This work involves 
numerical simulation of the performance of a sliding mode compensator scheme 
using a linear and a full 12 state nonlinear model of the equation of motion for 
the AUV. 


B. DESIGN OF SLIDING MODE COMPENSATOR 
1. Linear Model 
A State observer is designed which uses measured depth only in order to 
Бейтіаіс the vehicle pitch rate and pitch angle. The block diagram of the basic 
sliding mode compensator is illustrated in Figure 35 on page 71. The state ob- 


server design js based on the equation [Ref. 11]. 
t AC Ви К (С) (4.1) 


where x is the state variable estimated by the observer, and Ko 
Iecobserver feedback which is required for the observer to be able to follow 


and duplicate control] system operation. 
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A. B, and C are the original open-loop contro! matrices of 
equation (3.11). 


u is the sliding mode control law based on the observer. 


Note that in the sliding mode compensator, disturbance, nonlinear terms, and 
variation of parameters are ignored. Equation (4.1) is schematically depicted in 


Appendix A. Collecting terms and rearranging 
= (А-КС) + Ви+ Ку (4.2) 


which is then rearranged into the familiar state space form: 


г 
^ 


x= 44+ Ви 
у= СА+рИ (4.3) 


where ai is the observer A matrix (A—A,C) 
B is the observer B matrix 


C is the observer C matrix 


The values for observer feedback gain (A,) are calculated using (пе МО 
program “OBSERVER” in Appendix A. This provids three values of gain for the 
one state (depth) that 1s used by the observer in order to estimate the remaining 
two (pitch rate and pitch angle). The poles chosen for the observer are? 25d 
and -4.95. These values are selected because they create a faster response aime 
observer than in the controller itself. This condition is necessary to ensure that 
the observer will not slow down the overall simulation speed of the controller. 
The sliding surface of the sliding mode compensator has the same form as the 
sliding surface of a controller e(x) 2 S7x. 

The state observer provides necessarv information to the sliding mode controller. 
The control input of the sliding mode compensator can be described in the fol- 


lowing process. From the sliding condition we have 


--1 
— 
= 





eS 
OO а 


+ 


Figure 35. The block diagram of the sliding mode compensator. 
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d(t) 


Actual depth 


a(x) = —n*sign(c) (4.4) 
Substituting equation (4.3) to equation (4.4) and rearranging 


$1Х = —°5їеп(о) 


ST(A 4 Bu) = —1*sign(c) (4.5) 


Finally, the sliding mode control law can be expressed in terms of the estimated 
State variables in the following form: 


A 


ic zo By АЙЫ ene) (4.6) 
or 
u= Uti 


where и = -(578)-15747 
u — —(S? B) ysign(o) 


The Figure 36 shows the dynamic response of the vehicle with the зпаше оше 
compensator. The nonlinear feedback gain (4^) in the compensator is the same 
as the gain in the controller without the observer. It can De seen Mal amii 
Observer poles are fast. the performance of the current compensator resemble that 
of the controller alone of Chapter 3. A sliding mode compensator is easier to 


design and implement than conventional compensators. 
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Figure 36. The dynamic response of the vehicle with sliding mode compensator. 
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2. Nonlinear Model 
The designed nonlinear control feedback gain (17). sliding surface (o(x)), 
and observer feedback gain (Ky), which are based on the nominal linear model, 
are used to design a sliding mode compensator in order to handle a full nonlinear 
model. Since a nonlinear model can never be as easily predictable as a linear 
model, there exists certain degree of uncertainty in the nonlinear terms. A similar 
process aS in Chapter 3 provides a design of a nonlinear sliding mode 


compensator. The AUV nonlinear dynamics is given by 
X = Ax+Aflx)+B(x)u (4.7) 


where Afix), B(x) are the model errors of nonlinear terms and 


опсео 


Equation (4.7) can be rearranged by using the same proceedure in Chapter 3. 


Xš = Ах+АДх)+ВВи (4.8) 


The procedure for obtaining x(t) of x(t) is to compute the estimate to be the out- 


put of the dynamic system. 
A 


X = АХ+АДХ)+Ви+ Куу Сх) (4.9) 


We can rewrite (4.9) in the following state space form 


„= САн (4.10) 


The closed-loop dynamics of the state observer on the sliding surface with un- 
Certainty i$ given by 
6(5) = 515 
Т ^X ^ ^ 
= 5 ГАх+АДх)+рВи| (4.11) 


Substituting equation (4.6) to the above (4.11) for u, then we obtain the following 
equation 


се (1-7 s Ag STAR) 
— Pp ^ signe(X) (4.12) 


If ¢ < —nisigne(x) is enforced, stability will result. We can establish conditions 
on the use of 7° In equation (4.6) that will guarantee that sliding condition, given 


the bounds of uncertainty. It follows that 
п > Boe S AG) E (4.13) 
ich can be achieved by 
N? = Pmax (N+) | + | Brnax— 1) LESTAS | (4.14) 


These uncertainties in equation (4.10) are eliminated by using equation (4.6) but 
with large enough gain (#*) in order to guarantee stability. The boundary layer 
used in numerical simulation was selected in order to have the interpretation of 
smoothing out the discontinuity in the nominal control law at the switching sur- 


face. The nominal control law considered in this section. 


[m [stay 5745 5 ву serie = ) (4.15) 


e+ F(X) | + | (Bmax!) | STAX | 





where р 


-4 
сл 


guarantees asymptotic stability for the nonlinear model which have state observer 
as shown in Figure 37. The nonlinear feedback gain (5?) was chosen according 
to the assumed bounding nonlinear terms. The selected gain (i?) in the previous 
chapter is large enough to handle any unknown bounded uncertainty. The Fig- 
ure 37 depicts the expected robustness of the dynamic response using sliding 
mode compensator. This method produces an extremelv robust sliding mode 
compensator that performs predictably despite the use of the sliding surface, gain 
(1^). control law, and observer feedback gain (Kọ) which are based on the nominal 
nlinear model. The FORTRAN program “OBSERVER” in Appendix B has been 
written to implement the sliding mode compensator. The results of Figure 37 on 
page 77 were obtained bv using the standard hvdrodvnamic coefficients of the 


nonlinear model at 500 rpm. 
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Figure 37. The dynamic response of the sliding mode compensator for the standard 
AUV 
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C. -ROBUSTNESS TESIS 
1. Linear Model 

In the previous numerical analysis we saw that sliding mode compensator could 
deal with unknown nonlinear terms by using nonlinear feedback control law и. 
A modified system matrix A, and control matrix B is used to estimate robustness 
performance of the sliding mode compensator in this section. The effects of 
modeling error in a linear model can be compensated by considering the size of 
modeling error as in the classical state space methods [Ref. 11]. The variations 
in control matrix B can be handled using a similar method. This technique is easy 
to analvze for a single input svstem. The effects of modeling error can be consid- 
ered for any system using sliding mode compensator. Consider the effect of a 
Variation in the system matrix А and control matrix B. The system dynamics are 


given by 
х= (4+04)х+(В+0В)и (4.16) 


where oa is the unknown bounded changing svstem matrix 


OB is the unknown bounded changing control matrix 


The unknown changed system matrix (ôA) and control (0B) matrix can be ac- 
commodated bv using a large nonlinear (7°) and boundary layer (ф). The large 
gain Will guarantee stability and the boundary laver obtained in the previous 
chapter will eliminate the chattering which is intrinsicallv linked to the use of a 
switching surface. The FORTRAN program “OBSERVER” in Appendx A is 
used to estimate unmeasurable state and perform sliding mode compensator to 
the modified system. The same boundary laver (@). nonlinear feedback gain (7%), 
control law (4.6). and observer gain(A,) as in the nominal system are applied to 
the modified svstem to estimate the performance of a sliding mode compensator 
designed on a nominal linear model. The svstem matrix A and control matrix B 


are modified as in the following Table 6. 


Table 6. ROBUSTNESS TESTS CASE OF THE SLIDING MODE 
COMPENSATOR 


тав ОСЗ 
na | ^A? |B [ей — 









пе dynamic response for cach test ts shown in Figure 38 on page 80 through 
Figure 43 on page 85. Although the system matrix A and control matrix B are 
modified by 200°, there 1s only 10 ~ 11% overshoot to the test 5. The expected 
robustness of the dynamic response is presented by using sliding mode 
compensator based on nominal linear model. Sliding mode compensator per- 
 шапсс 15 үсгігсі for a linear vehicle with uncertain A and B matrix without 
considering additional design. 

The sliding mode compensator developed in this section was stable and insensi- 
tive to change in hvdrodvnamic coefficients of the AUV. It therefore appears that 
variable structure svstems will provide the most robust design for a sliding mode 
compensator that needs to maintain an accurate prediction of vehicle response 


under varving conditions. 
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Figure 38. The dynamic response of test 1 AUV (depth command = 100 ft) 
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Figure 39. The dynamic response of test 2 AUV (depth command — 100 ft) 
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Figure 40. — The dynamic response of test 3 AUV (depth command — 100 ft) 
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Figure 41. Тһе dynamic response of test 4 AUV (depth command — 100 ft) 
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Figure 42. The dynamic response of test 5 AUV (depth command = 100 ft) 
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Figure 43. The dynamic response of test 6 AUV (depth command = 100 ft) 


2. Nonlinear Model 
The siding mode compensator based on the nominal linear model is here applied 
to the highly nonlinear vehicle with modified hydrodynamic coefficients in order 
to estimate its robustness. 
The hydrodynamic coefficients, which affect in the depth change mancuvering, 
are modified as Table 5 in the previous chapter. Although an unmodeled dis- 
turbance could be made up for using model based compensator, parameter mis- 
match was determined to be sensitive to the used method [Ref. 12]. This would 
reduce the robustness of the controller when confronted with the varying, un- 
controlled condition found in the ocean enviroment. In this section, sliding mode 
compensator is used to investigate robustness for control of depth change ma- 
neuvering in the face of unmodeled nonlinear terms and parameter uncertainty. 


The nonlinear system dynamics for this purpose were described by 


x= Ax+dAx+f(x)+[B+6B+2(x) Ju (4.17) 


Since 6A, OB, fix), and g(x) are unknown values in the present system equation 
described above. they are assumed to be zero and y? is increased through numer- 
ical simulation in order to guarantce sliding mode control [Ref. 2]. The nominal 
control law (4.6) is used to simulate the response of all values (q, u, z, and s) both 
actual and estimated. The FORTRAN program "NSMC" was written in order 
to simulate numerical experiments provided in Appendix B. Note that in this 
section, a soft saturation function might be used to handle the highlv nonlinear 


model within the boundary layer. The soft saturation function is 


| 


“> 


Gis 


SF = | 4 





) x absc(X) (4.18) 
This saturation function produces soft inputs at approximately zero c(X) values. 


The dynamic response of the highly nonlinear AUV is shown in. Figure 44 on 


page 87 through Figure 49 on page 92. 
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Figure 45. The dynamic response of nonlinear test 2 AUV 
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Figure 46. The dynamic response of nonlinear test 3 AUV 
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Figure 47. The dynamic response of nonlinear test 4 AUV 
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Figure 48. The dvnamic response of nonlinear test 5 AUV 
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У. CONCLUSIONS AND RECOMMENDATIONS 


SUMMARY 


This thesis presented an application of variable structure systems for the ro- 


bust control of linear and nonlinear systems in the presence of disturbances and 


parameter variations. The designed sliding mode compensator based on this 


methodology was successfully emploved to provide a means for an unmanned 


underwater vehicle to control its depth under high nonlinearities. In this section, 


we Summarize the sliding mode compensator design of the specified AUV: 


ie 


to 


сл 


The fully nonlinear equations of motion are linearized and simplified (as il- 
lustrated in section 3.C) in order to produce linear equations in the state 
Space form. 


. The sliding surface, which provides desired dynamics of a closed-loop sys- 


tem. is designed by the standard pole placement method using the state 
Space form. 


. The shding mode control law, which consists of linear feedback and nonlin- 


ear switching feedback. can be obtained by satisfying the sliding condition. 


. Chattering due to piecewise discontinuous feedback is eliminated bv replac- 


ing the switched control law at the sliding surface by continuous variations 
across a thin boundarv laver neighboring the switching surface. 


. T he nonlinear feedback вап (у?) and boundary layer ($) are optimized Бу 


usenes of numerical experiments. 


The state observer is designed using classical methods. 


. The sliding mode compensator is designed by combining the sliding mode 


Control and state observer. 


CONCLUSIONS 


The objective of this work to design and analyze a sliding mode dive plane 


compensator for an autonomous underwater vehicle has been achieved. Realistic 


limitations due to pitch and pitch rate sensors non-availability were taken into 


ПЕКО. Ihe vehicle that was considered. the SDV is a typical AUV and its 


shape and characteristics greatly resemble the NPS vehicle. 


The conclusions of this work are summarized below: 


25 


— 


. A procedure for computing the sliding plane was established. The procedure 
is very general and can be applied to a large class of linearized single input 
svstems. Once the sliding plane coefficients have been determined, the 
switching feedback control law follows easily. 


ho 


Chattering problems, characteristic of variable structure system. were re- 
duced or eliminated by introducing a “boundary layer” in the switching logic, 
without violating the sliding condition. 


3. Robustness of the design was demonstrated by a wide variation of parame- 
ters of the linear model. Similar robustness characteristics were established 
when a full order observer was incorporated in the design in order to provide 
estimates for pitch angle and pitch rate based on depth measurements only. 


4. [he control design that was based on a linear model was tested against the 
full nonlinear equations of motion. Numerical simulations demonstrated the 
ability of the design to handle unmodelled dvnamics and variation of the 
hydrodynamic coefficients and geometric parameters of the vehicle. 


сл 


A visual simulation using an IRIS graphics workstation was used [Ref. 13] 
in order to view the dvnamic behavior of the AUV in real time under 
closed-loop sliding mode control. The vehicle response, Figure 50 on page 
95, was seen to follow the predictions that were based on the linear model. 


б. Finally. an experimental verification was attempted on the NPS protvype ve- 
hicle with coefficients the same as the ones used in [Кеб 12] ЛОБИ 
time sliding mode controller was designed based on 25 Hz sample rate with 
controller poles at [0.9. 0.9], 1 ], observer poles at [0:78, 0:792 0:80] on 
law 


u= —0.92—1 7890—21.2976n?sign(c) 
с = —0.33866 —1.68880--(z—2,,,,.) (5.1) 
КЕТЕ е = 0.2 апа — 1.0 


The experimental results are shown in Figure 51 on page 96, where the 
commanded depth was 5 volts (1 ft corresponds to 3.1 volts). 
The variable structure svstem was proven to be an attractive control system de- 
sign method for autonomous underwater Vehicles. The designed sliding mode 
compensator based on this methodology dealt with the dynamic problems of the 


underwater vehicle with ИОС теке 
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APPENDIX A. SIMULATION PROGRAM AND BLOCKDIAGRAM FOR 
LINEAR EQUATION 


А. MATRIX-X(SMC) OF THE LINEAR EQUATION 


k&%** MATRIX PROGRAM FOR SLIDING CONTROLLER  *** 


UX=6; // FORWARD SPEED 

СР1=0. 25; // CLOSED LOOP POLE 

CP2=0.27; // CLOSED LOOP POLE 

в, 2 // SLIDING SURFACE COEFFICIENT 
S2=(CP1+CP2); // SLIDING SURFACE COEFFICIENT 
S3-(CPl*CP2)/(-UX) // SLIDING SURFACE COEFFICIENT 
ШЕГІ 0,1:150.|”; 

IETA =6.0; 

GA3=-0.035; 

СА4=-0Х ; 

СА5=-0.7; 

САб=-0.03; 

СА7-52; 

GA8=S3; 

GA9-IETA; 

GA1021; 


GA1-(GA6-S3*UX)/(-GA3); 

СА2-(СА5-52)/(-САЗ); 

INPUT(:,1)=100*ONES(T); 

INPUT(:,2)=0.*ONES(T); 

Y=SIM(T,INPUT); 

PLOT(T,Y(:,1),'UPPER LEFT XLABEL/TIME(SEC)/ YLABEL/... 
DEPTH RESPOSE/TITLE/ DEPTH COMMAND- 100 ,STANDARD AUV/') 
ENCOT(T,Y(:,2),'UPPER RIGHT XLABEL/TIME(SEC)/ YLABEL/ ... 
PITCH ANGLE/TITLE/ CLOSED LOOP POLE=0.0,-0.35,-0.40/’ ) 
PLOT(T,Y(:,4),'LOWER LEFT XLABEL/TIME(SEC)/ YLABEL/ 

DIVE PLANE ANGLE(radins)/TITLE/ SPEED-26/') 
PLOT(T,Y(:,5),'LOWER RIGHT XLABEL/TIME(SEC)/YLABEL/ ... 
SLIDING SURFACE/TITLE/ DIS-0.2, B, NLINEAR,IETA-6.0,PI-0.4/') 


B. FORTRAN(SMC) OF THE LINEAR EQUATION 


Cauca CO COSA C) 


Go) 


720 
100 


Sevesleveveveseveseves eer таа гугу угуз УГУУСУ? ее: 


* “LINEAR AUV MODEL * 
* SLIDING MOCE CONTROL PROGRAM * 
* DEPTH CONTROL IN DIVE PLANE * 


* BY Кк RUNGE - n TIENE Mi * 


sesana 4: Je Je 272%: aoe) 22:92 Je ess ^o st 5: 27: yeye eve ove Je seve we ye ese eves 27: Vevey. 
7% 2. 2% 4. 72%... 572$ 2% 7% 9 4% 


REAL*8 TIME,QDOT, ZDOT, TDOT,THETA,ZPOS,Q,DS 
REAL*8 S, DE, UHAT, UBAR PHI АВ, БТА, СОЧ 
ити INITIAL CONDITIONS хижине 


TIMESOSODO 
Q=0. ODO 
THETA=0. ODO 
2Р05-0. ODD 
vuU SLIDING MODE CONTROL PARAMETER Тегесе 
EITA-4. ODO 
DELT=0. 02D0 
COMZ=100. ODO 
я 0. 400 
езе БУЗТЕМ РКОСКАМ “иж 
КЕТТЕ (8,710) 
DO 100 I=0, 6000 


FORMAT ЕТІ TIME’ ,5X,' DEPTH. wore. DIVE SS PITCH 
Ы SLIDE ) 


Орот--0. DO020, ODO THETA O OSD D5 
TDOT=Q 
ZDOT=— ОРО 


TIME=TIME+DELT 

Q=Q+DELT**QDOT 

THETA=THETA+DELT*TDOT 

ZPOS=ZPOS+DELT*ZDOT 

S=Q+0. 52D0**THETA-0. 0112D0**( ZPOS-COMZ) 

IF (ABS(S) .LT. PHI) SAT=(S/PHI) 

IP (S .LT. -PHI) SAT--1. ODO 

IP (S .GT. PHI) SAT=1. ODO 

ОНАТ=-5. 1229р0:0+1. 071400%ТНЕТА 

UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF (DE .GE. 0.4D0) DS=0. 4D0 

IF (DE .LE. -0.4D0) DS=-0. 4D0 

IF ((DE .LT. 0.4D0) . AND. (DE .GT. -0.4D0)) DS=DE 
WRITE (8,720) TIME, ZPOS, DS, THETA, S 

FORMAT (2X,E11. 3,4X,E11. 354% Blue.) bien ОШ 
CONTINUE 

STOP 

END 
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С. 


MATRIX-N(OBSERVER) OF THE LINEAR EQUATION 


*** SIMULATION PROGRAM FOR SLIDING MODE СОМРЕМЗАТОВ *** 


UX=6; // FORWARD SPEED 

(PI 0.3 // CLOSED LOOP POLE 

CP2=0.40; // CLOSED LOOP POLE 

Sl=1.; // SLIDING SURFACE COEFFICIENT 
S2=(CP1+CP2); // SLIDING SURFACE COEFFICIENT 
S3=(CP1*CP2)/(-0.6); // SLIDING SURFACE COEFFICIENT 
ЕІТА -6.0; 


A-[-0.7,-0.03,0;1,0,0;0,-6,0]; 
B2[-0.035;0;0]; 


Ем|0,0,1); 

Dc VAC*C',A' *A' *C'] 

ЕО. 7,0.03;0,1,0.7;0,0,1]; 

ША! -5. 5: // DESIRE POLE OF OBSERVER 
ШЕ2-5,7/75; ⁄⁄ DESIRE POLE OF OBSERVER 
LA3=5.9; // DESIRE POLE OF OBSERVER 


ABAR=[0.7;0.03;0]; 
AHAT=[LA1+LA2+LA3;LA1*LA2+LA2*LA3+LA1*LA3;LA1L*LA2*LA3]: 
KC-INV((N*W)')*(AHAT-ABAR); 


СА1=-1.4; 
GA2=-0.06; 
GA3=-2*UX; 
EXT-KC(3,1); 
БЕ5-КС(2,1); 
САб-КС(1,1); 
СА7=-0.7; 
СА8--0.03; 
СА9--6; 
СА10--0.035; 
СА16--0.035; 
БЕШПІ-0,75:; 


СА13=-0.0233; 

GA14=(GA2-S3*UX) /(-GA10); 

GA12=(GA7+S2)/(-GA10); 

GA15=EITA; 

Ш-|0:0.1:150.|”: 

ШПЕОТ(:,1)-100%ОМЕ5(Т); 

ШШЕТТІ( :,2)-0.0%0МЕб5(Т); 

Y=SIM(T, INPUT); 

OT, {Y(:,1) Y(?:,5)]; LINE STYLE 1 2 /UPPER LEFT XLABEL/ 
TIME(sec) / YLABEL/EST AND ACT DEPTH(ft)/') 

EmOTT [Y(*:,2) Y(:,4) ]N; LINE STYLE 1 2 /UPPER RIGHT XLABEL/ 
TIME(sec)/ YLABEL/EST AND ACT ANGLE(rad)/TITLE/ B, 2*A/') 
peo To Y(:,3),’ LOWER LEFT ALABEL/TINE(SEC)/ YLABEL/ 

DIVE PLANE ANGLE/TITLE/ S.F.C-1, 0.75. -0.0233> SPEED=6/' ) 
Bpmori tT, ¥(2,6),’ LOWER RIGHT XLABEL/TIMNE( SEC) /YLABEL/ 
SLIDING SURFACE, TITLE/ SAT, DISS, БЕЛГІ PI=0 47°) 
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D. FORTRAN(OBSERY LR) "n THE LINEAR EQUATION 


еб O. 


COO 


ое и 
А LINEAR AUV MODEL * 
Ы SLIDING MODE CONTROL PROGRAM * 
* DEPTH CONTROL IN (Di VE ee BANE * 
* к. USING EULER METHOD * 


9:22929:9:9(22:2252227699:925692929796969:5:7965692269636529696996369696569692 


REAL*8 TIME ОБОИ ОИЕ ВСИ РОО DS 
REAL*8 S, DE, UHAT, UBAR,PHI,SAT,EITA,COMZ 
REAL*8 THADOT,OQHADOT, ZHADOT , QHAT , THAT, ZHAT 
orotate INITIAL CONDITIONS erer 


TIME=0. ODO 
Q=0. ODO 
THETA=0. ODO 
ZPOS=0. ODO 
QHADOT=0. ODO 
THADOT=0. ODO 
ZHADOT=0. ODO 
QHAT=0. ODO 
THAT=0. ODO 
a ODO 
wade SLIDING MODE CONTROL PARAMETER 5253575 
EITA- 4. 000 
DELT=0. 02D0 
COMZ=100. ODO 
PHI=0. 4D0 
тте: 6Ү5ТЕМ РРОСРАМ 757995799 
WRITE (8,710) 
DO 100 I=0,6000 


FORMAT (310€ TINE. SX, a DEPTH ETON DIVE "one PITCH 


E: SLIDE') 


ОрОТ--0. 3500%0-0. 01500%ТНЕТА-0. 017500%05 
TDOT=0. Opo 
ZDOT--6. 0DO*THETA 


TIME=TIME+DELT 
Q=Q+DELT*QDOT 
ТЕЕ ВЯ ЕРЕ TUOT 
я ZPOSTDELT*ZDOT 
<> SUBROUTINE OBSERVER ES 
CALL OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT ) 


Te SLIDING MODE INPUT VYeze%ese7egezeze 


S=QHAT+0. 52D0*THAT-0. 0112D0**( ZHAT-COMZ ) 
IF ЦАВ5(5) .LY. PHD) ЗАТЕ РНЕ) 

ТР БІЗ PHI) SATSI 

IF (S OCTA PHI) БАТЕ ODO 

UHAT=-5. 1429D0**QHAT+1. 0714D0**THAT 


100 


720 
100 


UBAR=EITA*SAT 

DE=UHAT+UBAR 

TP (VE СЕ. 0. 850) 05=09. 490 

IF (DE .LE. -0.4D0) DS=-0. 4D0 

Ее РЕ. Г. О. 400) AND: (DE CT. О 580) № ВЕБЕ 
IIIE CSE 2U) TINE, 2205, 55. THETA STS 

ESRIATEC2X ETT. 3, AX; ETT. 3, AX, E11. 3; 4X, ETT: 3, 4 ES) 
CONTINUE 

EN OP 

END 

зто кий SUBROUTINE OBSER rririk 


SUBROUTINE OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT , ZPOS , DELT) 
QHADOT=-0. 7D0*QHAT-0. 03DO* THAT - 20. 9293D0*( ZPOS - ZHAT) 
THADOTZQHAT - 14. 4092DO*( ZPOS -ZHAT) 

ZHADOT=-6. ODO*THAT+16. 45D0*( ZPOS~ZHAT ) 


QHAT=QHAT+DELT*QHADOT 
THAT=THAT+DELT*THADOT 
ZHAT=ZHAT+DELT*ZHADOT 
RETURN 

END 


E. FORTRAN GRAPH 


DIMENSION TIME(6000),DEPTH(6000),DIVE(6000),PITCH(6000), 
*SLIDE(6000) 

REAL TIME,DEPTH,DIVE,PITCH,SLIDE 

DO 1 I=1,6000 
1 READ (8,*) TIME(I),DEPTH( I) ,DIVECI),PITCH(I),SLIDE(I) 
CALL COMPRS 


C wv CREATES A DUMMY PLOT FOR POST PROCESS ING*# xs 


CALL NOBRDR 

CALL AREA2D(1. ,1. ) 

CALL CRAG TS OO TD 
CALL ENDPL(0) 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


C Heee PLOT COMMAND DEPTH Keser 


PAGE(3. ,4. ) 

NOBRDR 

AREA2D(2. 3,3. 0) 
XNAME( ' TIMECSEC)$',100) 
YNAME('COMMAND DEPTH(FT)$',100) 
THKFRM(O. 03) 

БРАВО 90810025192 5:907: 595 
THKCRV(0. 02) 

CURVE( TIME , DEPTH, 6000,0) 

ENDPL(0) 


еезек PLOT DIVE PLANE ANGLE ete: 


PAGE(3. ,4. ) 

NOBRDR 

AREA2D( 2. 3,3. 0) 
XNAME( ' TIME( SEC)$' , 100) 

YNAME('DIVE PLANE ANGLE(RAD)$',100) 
THKFRM(0. 03) 

СВАР(О. ‚30.0, 120. ,-.4,.2,.4) 
ТНКСВУС О. 02) 

CURVE( TIME , DIVE ,6000,0) 

ENDPL(0) 


JT PLOT PITCH ANGLE © те 


PAGES 4.) 

NOBRDR 

АКЕД2 3.3.0) 

XNAME( TIME(SEC)S ',100) 
YNAME('PITCH ANGLE(RAD/SEC)S',100) 
THRERAC 0. 03) 

ОНЫН Пат Пе Он Оса Ока) 
ТНКСКУ(О. 02) 

CURVE TINE FITC SOO) 

ENDPL(0) 


wwe PLOT SLIDING SURFACE И сс с. 


CALL 
CALL 
CALL 
CALL 
C А LL 


PAGE(3. ,4. ) 

NOBRDR 

АКБАОП(2 2288. 

XNAME(' TIMECSEC)S' , 100) 

YNAME('SLIDING SURFACE(RAD/SEC)S',100) 


ШО 


CALL THKFRM(0. 03) 

Il CIAO 30. 00120. 050.515) 
CALL THKCRV(0. 02) 

CALL CURVE(TIME,SLIDE,6000,0) 

CALL ENDPL(0) 


CALL DONEPL 
STOP 
END 


F. BLOCK DIAGRAM OF THE MATRIX-X SIMULATION 





Figure 52. The block diagram of the SMC simulation 
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APPENDIX B. SIMULATION PROGRAM FOR NONLINEAR AUV 


- ег ela nl anta n a 
FU sv ON dv Фу 99 


yeee жу: 
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av ev er 


yeyeyeeyedededeyesedede ez edesegedeJesesedeyeedededesesevezgededevedeJedededesyededededesteedeseseJe2ese 
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NONLINEAR AUV MODEL / STERN PLANE AND BOW PLANE SEPARATED * 


VARIABLE DECLARATION 11 
CONSTANTS 
INITIAL CONDITIONS 


MASS 


REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 
REAL 


ПАША, 
REAL 


vee INPUTS 2 248 

PROPULSION MODEL 271 

х О0ТРОТЅ 423 
INTEGRATION 


70 
128 
206 
245 
2: 2: we Je г ri Зе eye Zee m s Je е: 3: JeeJee Je 222 yes 4: г Je 2: st: we sje eves PIP levers deve ег уу: “зе جو‎ 
ANCB2, 82) 
MASS , LATYAW ,NORPIT 
MM(6,6) ,G4(4) ,GK4(4) , BR(4) , HH(4) 

B(6,6),BB(6,6) 
AI 
XPP ,Х00 ,XRR ,XPR 
XUDOT ,XWQ ,XVP ,XVR 
XQDS ,XQDB ,XRDR ,XVV 
XW ,XVDR ,XWDS ,XWDB 
XDSDS,XDBDB ,XDRDR ,XQDSN 
XWDSN ,XDSDSN 
TIME,S,EITA,UBAR,UHAT,COMZ,BAR,SIM,DE,SAT 


MATRIX 


УТУУ 


LATERAL HYDRODYNAMIC COEFFICIENTS 


REAL 
REAL 
REAL 
REAL 


YPDOT ,YRDOT,YPQ  ,YQR 
YVDOT ,YP  ,YR  ,YVQ 
YWP ,ҮМЕ ,YV ,YVW 

ҮРЕ CDY 


NORMAL HYDRODYNAMIC COEFFICIENTS 


REAL 
REAL 
REAL 
REAL 
REAL 


ROLL 


REAL 
REAL 
КЕЛІ, 
КЕЛІ, 


ZQDOT ,ZPP,ZPR ,ZRR 
ZWDOT ,ZQ „ZVP  ,ZVR 
ЛЫ 2775 ZDE 
ZQN ,ZWN ,ZDSN ,CDZ 
ZHADOT , ZHAT 


HYDRODYNAMIC COEFFICIENTS 


KPDOT ,KRDOT ,KPQ ,KQR 
KVDOT , KP ,KR KQ 
KWP , KWR „KV „Кук 
KPN , KDB 


PITCH HYDRODYNAMIC COEFETCPENIS 


REAL 
REAL 
REAL 
REAL 


ODOT HERE UER MRK 
ЕВО О АЕ УК 
Мы, МУҰ SMDS. -MDE 
MQN , MWN  ,MDSN 
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REAL QHADOT,QHAT,THADOT, THAT 
YAW HYDRODYNAMIC COEFFICIENTS 


REAL NPDOT,NRDOT,NPQ ,NQR 


REAL NVDOT , NP SS ‚МУО 
REAL NWP , МЕ „МУ , NVW 
REAL NDR 


MASS CHARACTERISTICS OF THE FLOODED VEHICLE 


REAL WEIGHT , BOY „VOL ‘XG 

FEAL YG , ZG ХВ ZB 

REAL IX , IY 217 D 

REAL ІҮ2 , IXY ,YB 

REAL L , RHO ,G NU 

REAL AO  ,KPROP ,NPROP , X1TEST 

REAL DEGRUD  ,DEGSTN 

COMMON /BLOCK1/ F(12), FP(6), XMMINV(6,6), UCF( 
INTEGER N,IA,IDGT,IER,LAST,J,K,M, JJ, KE,I 

REAL — WKAREA(54), X(12) 


RUDDER COEFFICIENTS 
РӘКАМЕТЕК ( БӘМАХ- -0. 175) 
LONGITUDINAL HYDRODYNAMIC COEFFICIENTS 


PARANETER(XPP = 7.E-3 ,XQQ = -1.5E-2 ,XRR = 4. 
-3 


& XUDOT=-7.6E-3 ХКО = -2.E-1 ,XVP = -3.E 
 Х0р$=2. 5Е-2 ,XQDB=-2. 6E-3 ,XRDR= -1.Е-3 
omen, =1. 7E=1 ,XVDR=1. 7E-3 , XWDS=4. 6E -2 

& XDSDS= -1.Е-2 ,XDBDB= -8.E-3 ,XDRDR= -1.E-2 
& XWDSN=3.5E-3 ,XDSDSN= -1.6E-3 ) 


LATERAL HYDRODYNAMIC COEFFICIENTS 


BARAMETERCYPDOT=1..2E-4 ,YRDOT=1. 2E-3" ,YPQ = 
БЕ ШОТ--5. 5Е-2 ‚УР = SED КЕ 52522 
ИР =2. ЗЕ -1 INR ll 9E SE 
ВБК —2, 76-2 БЕЗ ви) 


NORMAL HYDRODYNAMIC COEFFICIENTS 


ESDAMETERCZODOT--6.8E-3 ,ZPP -1.3E-4  ,Z2PR 26.7 


& ZWDOT--2. 4E-1 ,20 т-1.4Е-1 AVP =-4 8E- 2 
И = -5.Е-1 eh Vi =O Oka j 4U => 7. pes 
EN =-2. 9E=3 ANN SSIES ADSM SIE 


ROLL HYDRODYNAMIC COEFFICIENTS 


PARANETER(KPDOT= -1.Е-3 ,КЕрОТ--3.4Е-5 ,КРО --6 
& KVDOT=1.3E-4 | , КР --1.1Е-2 ‚КВ =-8. 4Е-4 
& КИР =-1.3Е-4 , KWR =1.4E-2 ‚КУ =3. 1Е-3 
ВЕЕ ОБЕ KDB = 0,0 ) 


4) 


Е-3 ,XPR =7.5E-4, 


‚ХУВ = 2.Е-2, 
,XVV =5. ЗЕ-2, 
,XWDB= 1.E-2, 


OUD EI) 


4.Е-3 ,YOR =-6.5E-3, 


,YVQ =2. 4E-2, 
,YVV =6. 8E-2 , 


Е-3 ,ZRR =-7.&E-3, 
,ZVR =4. 5E-2, 
,ZDB =-2. 6E-2, 
CD2 10) 


.9E-5 ‚КОВ =1. 7Е-2, 
‚КУО=-5. 1Е-3, 
‚КУК =-1. 9E-1, 


PITCH HYDRODYNANIC COEPPICIENYs 


PARAMETER( MQDOT-- 1. 7E-2 ,HPP =5.3E-5,MPR = 5.EZSSHRR 52249 LM 
& MWDOT=-6.6E-3 , МО =- Ci? MVE SIPE „МУК —3247 B2 
& MW = 1.Е-1 ‚ MVV SAMOE =Z „MDS =-4. 1Е-2 ‚ОВ Е, Ен 
& MON 5S1 6ES „Мик =2. 9ES ПРМ чо. 26) 


YAW HYDRODYNAMIC COEFFICIENTS 

PARAMETER(CNPDOT=-3. 4E-5 ,NRDOT=-3. 4E-3,NPQ 2-2- TE-2 (NOR = Е 
& NVDOT=1. 2E-3 , NP =-8.4E-4 NR = СЕЕ | NVQ = -1. E= 25 
& NWP SVE » NWR =7. 4E-3 ‚МУ =-7. 4Е-3 „КУИ <-2 Па 

& NDR = ВЕ) 

MASS CHARACTERISTICS OF THE FLOODED VEHICLE 


PARAMETER( WEIGHT =12000. , BOY =12000. ,VOL =200. ,XG = 0. 


3 


& ҮС = 0.0 , Z8 2 0.2 „ХВ = 0. ОВЕ осии 
SI = S00, , IY = 10000. 19 - 000%, И 

& IYZ = -10. Приета VOCE 

& L= 17.4 ‚ ВНО = 1.94 „С Ево ‚№0 = 8.47Е-4 , 
S WD - 2.0 ,KPROP = 0. ,NPROP = 0. , XITEST= 0.1 , 
& DEGRUD- 0.0 ,DEGSTN= 0.0) 


INPUT INITIAL CONDITIONS HERE IF REQUIRED 


00 
VO 
WO 
PO 


гә 
e 


I, @ O O O O — 
© ее (со е) Цао Две ав и 


APOS = XPOSO 
TEOS = YPOSO 
ZPOS = ZPOSO 
PSI = PHIO 
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ТНЕТА = ТНЕТАО 
FHI = PHIO 
QHADOT=0. 0 
THADOT=0. 0 
ZHADOT=0. 0 
QHAT=0. 0 
THAT=0. 0 
ZHAT=0. 0 


DEFINE LENGTH FRACTIONS FOR GAUSS QUADRATURE TERMS 


G4(1) = 0. 069431844 
G4(2) = 0. 330009478 
G4(3) = 0.669990521 
G4(4) = 0.930568155 


DEFINE WEIGHT FRACTIONS FOR GAUSS QUADRATURE TERMS 


GK4(1) = 0. 1739274225687 
GK4(2) = 0.3260725774312 
GK4(3) = 0. 3260725774312 
СКА(4) = 0. 1739274225687 


PEPINE TRE BREADTH BB AND HEIGHT HE TERMS FOR THE INTEGRATION 


БЕ 1) = 757/12 
ЕР(2) = 75.7/12 
BR(3) = 75.7/12 
BR(4) = 55.08/12 
Nu ) - 16.38/12 
НН(2) - 31.85/12 
I(3) = 31.85/12 
HH(4) = 23.76/12 


MASS = WEIGHT/G 


N= 6 
DO 15 J =1,N 
DO 10 K = 1,N 
XMMINV(J,K) = 
MM(J,K) = 0.0 
10 CONTINUE 
15 CONTINUE 


0.0 


MMC 1,1) = MASS -((RHO/2)*( L***3)*XUDOT) 
| (1.5) = 1455-26 
Mit 1s6)°= Nass YG 
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ММ(2,2) = МА$5 -((ЕНО/2)*%(1®*3)*үүроТ) 
MM(2,4) = -MASS*ZG -((RHO/2)%*( L¥*4)*YPDOT) 
MM(2,6) = MASS*XG - ((RHO/2)*( L**4)*YRDOT) 
MM(3,3) = MASS - ((RHO/2)*(L**3)*ZWDOT) 
MM(3,4) = MASS*YG 

MM(3,5) = -MASS*XG -((RHO/2)%*( L**4)*ZQDOT) 
MM(4,2) = -MASS*ZG - ((RHO/2)*(L'*4)*KVDOT) 
MM(4,3) = MASS*YG 

MM(4,4) = IX - ((RHO/2)*(L**5 )*KPDOT) 
MM(4,5) = -ІХҮ 

MM(4,6) = -IXZ -((RHO/2)*( L**5 )*KRDOT) 
MM(5,1) = MASS*ZG 

ММ(5,3) = -MASS*XG -((RHO/2)*(L»*4)*MWDOT) 
ММ(5,4) = -ІХҮ 

MM(5,5) = IY -((RHO/2)*(L**5)*MQDOT) 
MM(5,6) = -IYZ 

MM(6,1) = -MASS*YG 

MM(6,2) = MASS*XG -((RHO/2)*( L'**4)*NVDOT) 
MM(6,4) = -IXZ - ((RHO/2)*(L**5)*NPDOT) 
MM(6,5) = -IYZ 

MM(6,6) = IZ - ((RHO/2)*( L**5 )*NRDOT) 


LAST = N*N+3*N 

ПЕ Е Ел 

WKAREA(M) = 0.0 
20 CONTINUE 


IER = 0 

IA = 6 

IDGT = 4 

CALL LINV2F(MM,N,IA,XMMINV, IDGT,WKAREA, IER) 


житие еее та INPUTS ИРИС 
RUDDER AND DIVE PLANE COMMANDS 


DELT=0. 1 
SIM= 800.0 
TIME= 0.0 
DS= 0.0 
DR= 0.0 
DB= 0.0 
RPM=1000. 0 
EITA=6. 0 
GO o 
BAR=0. 4 
c SIZE OF OUTPUT DATA ARRAY FOR PLOTTING 
c NUMOUT=6 
с NUMPNT=4 
с WRITE (8,711) NUMPNT,NUMOUT 


WRITE (8,710) 
711 FORMAT (214) 
DO 100 I=1,SIM 


OBO OO OO 


PROPULSION MODEL 


SIGNU = 1.0 

ПЕСО БТ. 9. 0) 5169 = -1.0 
АВЕ CI XITEST) = XITEST 
SIGNN = 1.0 

TT (REV ET OO) SION =5 10 

ETA = 0. 012*RPM/U 

RE = U*L/NU 


CDO 
CT 

CIT 
EPS 
ХРК 


ОР 


ОО РОЗ ПЕ 2E 2 


0. 008%12е%2%ЕТА%АВ5(ЕТА)/(А0) 
0- 00851252 /(А0) 
-1. 0+SIGNN/SIGNU*( SQRT(CT+1. 0)-1. 0)/(SORT(CT141. 0)-1. 0) 


CDOSGETASABSCETA) - 1.0) 


CALCULATE THE DRAG FORCE, INTEGRATE THE DRAG OVER THE VEHICLE 
INTEGRATE USING A 4 TERM GAUSS QUADUTURE 


LATYAW 


NOR 
DO 


E 
C 
C 
C 
C 
C 
C 


PIT 
500 


Н 
oo 
EO 


4 


UCF(K) 2 SQRT((V4GACK)*R*L)s*2 4 (W-GACK)*Q*L)99*2) 
IF(UCF(K).GT. 1E-10) THEN 


TERMO 


TERNI 
ПЕ 2 
ГАТУАМ 
NORPIT 


END 


( RHO/2)* (CDY*HH(K)*( V£GA(K)*R*L)s*2 + 
CDZ*BR(K)*(W-GA(K)*Q*L):92) 

TERMO* ( V-GA( K)*R*L) /UCF( K) 
TERMO*(W-GA(K)*Q*L) /UCF(K) 

LATYAW + TERM1*GK4(K)*L 

NORPIT + TERM2*GK4(K)*L 


lI H H Il 


TE 


00 CONTINUE 


FORCE EQUATIONS 


LONGITUDINAL FORCE 


ЕР(1) = MASS*V*R - MASS*W*Q + MASS*XG*Q**2 + MASS*XG*R**2 - 


R wm p» QE En pn 


MASS*YG*P*Q - MASS*ZG*P*R + (RHO/2)*L**4*(XPP*PA2 + 
Х00%0%%2 + XRR*R?772 + KPRP R) t(RHO/2)7*L7**3*( XWQ*W*Q + 
XVP*V*P-XVR*V«*RAUA*QA*(XQDS*DS-TXQDBA*DB)-TXRDR"U*RADR)- 
(RHO/2)7*L7**2*(XVV*V*72 + ХИМИИ" + XVDR*U"VeDR + U*xWr 
CAWDS*DS+XWDB*DB )+U***2*( XDSDS*DS**2+XDBDB*DB**** 2+ 
XDRDR*DR**2))-(WEIGHT -BOY)*SINCTHETA) +(RHO/2)*L**3* 
XQDSN*U*Q*DS*EPS-T(RHO/2)*L**2*( XWDSN*USW*DSTXDSDSN*UN 27 
DS**2 D ЖЕР5 +( КНО/ 2 )"L** 2*U*72**XPROP 


11] 


LATERAL FORCE 


FP(2) = -MASS*U*R - MASS*XG*P*Q + MASS*YG*R**2 - MASS*ZG*Q*R + 
& (RHO/2)*L**4*(YPQ*P*Q + YQR*Q^*R)T(RHO/2)*L**3*(YP*U*P + 
& YR*U*R + YVQ*V*Q + YWP*WeP + YWR*W*R) + (RHO/2 )*L**2% 

& CYV*U*V + ууухуху +YDR*U%*2*DR) -LATYAW +( WEIGHT-BOY)* 
& COS( THETA)*SIN( PHI )+MASS*W* P+MASS*YG* P¥%* 2 


NORMAL FORCE 


FP(3) = MASS*U*Q - MASS*V*P - MASS*XG*P*R = MASS*YG*Q*R + 
MASS*ZG*Pp**2 + MASS*ZG*Q**2 -- (КНО/2)*%*[**%4&%(7РР*Рр**2 + 
ZPR*P*R + ZRR*R**2 ) + ( RHO/ 2) * L*** 3*( ZQ*U*Q - ZVP*V*P + 
ZVR*V*R) Tí RHO/2 )*L**2*( ZW*U*W + ZVVEVERD + 2%( ZDS* 
DS+ZDB*DB ) ) -NORPIT+( WEIGHT -BOY )*COS( THETA )*COS( PHI )+ 
( RHO/2)*L**3*ZQN*U*Q*EPS T(RHO/2)*L95*2**( ZWN*U*W -ZDSN* 
U***2*DS)*EPS 


оз өз сз оз оз оз 


ROLL FORCE 


ЕР(4) = -12%0%К TIY*Q*R -IXY*P*R +IYZ*Q**2 -IYZ"*R**2 +IXZ*P*Q - 
MASS*YG*U*Q -MASS*YG*V*P -MASS*ZG*W*P+( RHO/2 )*L**5*( KPQ* 
P*Q T KQR*Q*R ) T(RHO/2)*L**4*(KP*U*P +KR*U*R + KVQ*V*Q اك‎ 
KWP*W*P + KWR*W*R ) +(RHO/2 )*L**3*( KV*U*V + KVW* V*W ) - 
(YG*WEIGHT - YB*BOYO*COSCIHETAYSGOSCPHTOSS CZGO WENNS HE 
ZB*BOY)*COS(THETA)*SIN(PHI) + (RHO/2)*L**4*KPN*U*P*EPS+ 
(ВНО/2) Ти 30 и2*КРЕОР +М4А$5*76*0*В 


@ @ © р (р RF 


PITCH EFORCE 


ЕР? - -IX-*P*R +IZ*P¥R TIXY*Q*R -IYZ*P*Q -IXZ*P**2 cIXZ*R?*-2 - 
MASS*XG*U*Q € MASS^XG*V*P ғ МА55%246%УЗК - MASS*ZG5*W^Q + 
( RHO/2)*L**57*(MPP*P7*7*2 TMPR*P*R TMRRURAS*2)-(RHO/2)*L**4* 
(MQ*U*Q + МУРхужр + MVR*V*R) + (RHO/2)*L**3*(MWsU-W + 
MVV¥ V2" 24U%% 2% ( MDS*DS+MDB*DB) )+ NORPIT -(XG*WEIGHT- 
XB BOY )*COS( THETA )*COS( PHI )+(RHO/ 2 )*L**4%MQN*U*Q*EPS + 
(RHO/2)*L**37( MWN*U*W+MDSN*U**2*DS ) “EPS - 
( ZG**WEIGHT-ZB*BOY )*SIN( THETA) 


готово 


YAW FORCE 


FP(6) —- -IY*P*Q TIX*P*Q TIXY*P*-2 -IXY*Q**2 TIYZ*P*R -IXZ*Q*"R - 
MASS*XG*U*R + MASS*XG*W*P - MASS*YG*V*R + MASS*YG*W*Q + 
( RHO/2)*L7*7*5*(NPQ*P*Q + NQR*Q*R) t( RHO/2)*L**4*(NP*U* Pr 
NR*U*R + NVQ*V*Q TNWPTWATP + NWR*W*R) T(RHO/2)*L7**3*(NV* 
U*V * NVW-*V*W 4 NDRA*U**2*DR) - LATYAW + (XG*WEIGHT - 
XB*BOY)*COS(THETA)* SINCPHI)T(YG*WEIGHT)*SIN(THETA) 
tCRHO/2)5)*L55*345U**2«*NPROP-YBSBOYSSDINCDHETAS 


@ @ @ @ fe 


NOW COMPUTE THE F(1-6) FUNCTIONS 


DO 600 J = 1,6 
F(J) = 0.0 
DO 600 K = 1,6 


F(J) = XMMINV(J,K)*FP(K) + F(J) 


600 CONTINUE 


THE LAST SIX EQUATIONS COME FROM THE KINEMATIC RELATIONS 


FIRST SET THE DRIFT CURRENT VALUES 


UCO = 0.0 
VEO = 0.0 
WCO = 0.0 


ІШЕК ТАН РОЗІТТОК КАТЕ5 F(7=9) 


F(7) = UCO + U*COS(PSI)*COS( THETA) + V*(COS(PSI)*SIN(THETA)* 
& Ош ВИЕ yeas SINCESI)*€OSCPHI = WeGeOs( PS) SINGHHETA)* 
& COS CEI ST O INCPSI)#>SINCPHI)) 


PCS) = VCO T USGSINCPSI)#COSC(CTHETA) + \* 
& В EOS PS TACOS PHT) + 
& COS PHIDE EC OSOCPSI)*SINCPHT)) 


ВИ Е е 
W*(SIN(PSI)*SIN(THETA)* 


nas — WCO - U*SINC THETA)  +V*COS( THETA)*SINC PHI) +W*COSCTHETA)* 
& 


COS( PHI) 


EULER ANGLE RAGES F(10=12) 


F(10) = P + Q*SIN(PHI)*TAN(C THETA) + R*COS(PHI)*TAN( THETA) 


II 


Dd 


Q*COS(PHI) - R*SIN(PHI) 


Е(12) = Q*SIN(PHI)/COS(THETA) + R*COS( PHI) /COS( THETA) 


UDOT = F(1) 
VDOT = F(2) 
WDOT = F(3) 
PDOT = F(4) 
QDOT = F(5) 
RDOT = F(6) 
peor = F(7) 
YDOT = F(8) 
ZDOT - Е(9) 


PHIDOT + Е(10) 
ИНЕТА = FCI) 
PSIDOT = F(12) 


CCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCCC 


ә ер Бері Фр 


CREATE OUTPUT DATA FILE 


TESTER=MOD( FLOAT( I), 100. ) 
IE ESTER Еб. 


0.0) THEN 


TIMER=PLOAT( 1)/2. 


WRITE (8,730) 
WRITE (8,730) 
WRITE (8,730) 
WRITE (8,730) 
WRITE (8,730) 


BOOS OS 


DS ,DR,XPOS,YPOS ,ZPOS, ROLL, PITCH, YAW 
FP(5) 

U,V,W,P,Q,R 

пин n m 

MASS NC LG 


WRITE (8,730) RHO,L,MPP,MPR,MRR 
WRITE (8,730) MQ,MVP,MVR,MW 
WRITE (8,730) MVV,MDS,DS,MDB,DB,NORPIT,WEIGHT 
WRITE (8,730) XB,BOY,THETA, PHI ,MQN,EPS 
WRITE (8,730) MWN,MDSN,ZB 
WRITE (8,730) TIME,DEPTH,DS,THETA,S 
20 FORMAT (1X, ‘TIME’ , 3X, ' COMMAND DEPTH’ ,3X, ‘DIVE P ANG', 
*3xX,'PITCH ANGLE' ,3X, SLIDING SURFACE’ /) 
C30 FORMAT (1X,F6. 2,2X,E11.3,3X,E11.3,4X,E10. 3, 3X 
710 FORMAT (1X, — TIME ШО 0 eh eee 
ЖЗ Dom) 
WRITE (8,730) TIME,U,ETA,XPROP,DS 
730 FORMAT (1X,F6.2,2X,F6.2,2X, F6. 3,2 TN NI 
С ENDIF 
CCCCCCCCCCCCCCCCCCCCECCCECCCCCCECCTEGE TE CO CE OOO 


ATO GOO OO 


E11. 3) 


! XPROP', 


C FIRST ORDER INTEGRATION 

C 
U = U + DELT*UDOT 

C U = SURGE RATE 
А ЗО 

С V = SWAY RATE 

W = W + DELT*WDOT 

С W = HEAVE RATE 
P= PADEC PEDON 

C P = ROLL RATE 
Q =O ШЕН oper 

С Q = PITCH RATE 
R = R + DELT*RDOT 

C R = YAW RATE 
XFOS = XEOS +. DELI ВОТ 

C Хх = SURGE 
YPOS - YPOS ШЕТУ 

C Y = SWAY 
2Р05 = 2805 ВЕСЕЛО 

С Z = HEAVE 
PHI 5= PHI ОЕ ЕНІ ВОТ 

С PHI = ROLL 
THETA = THETA + DELT*THETAD 

C THETA = PITCH 
PSI = PSI + DELT- PSIDOT 

C PSI = YAW 

C 

C Тетелес: OBSERVER OF SLIDING CONTROLLER 777 тт т тс 
CALL OBSER(QHADOT, THADOT, ZHADOT, QHAT, THAT, ZHAT,DELT, ZPOS,DS,UO0) 

C 

C 

[e erer SLIDING MODE CONTROL INPUT eveye 


S=QHAT+0. 75*THAT-0. 0233*( ZHAT-COMZ) 

IF (ABS(S) „ПТ. BAR) SAT=(S/BAR)*ABS(S) 
ТЕБ ЕЕ) AT 

IF (SURGE. BAR Cdl 

UHAT-1. 4286*QHAT43. 1429* THAT 
UBAR=EITA*SAT 

DE=UHAT+UBAR 

IF (DE .GE. 0.4) DS=0.4 


100 


ОВЕ ШІ -0.4) 05=-0 4 

IF( (DE .LT. 0.4) . AND. (DE .GE.-0.4) ) DS=DE 
DB--DS*1.0 

TIME=TIME+DELT 


PHIANG 
THEANG 
PSIANG 


EHE 0. 0115522925 
THE TAO. 0174532925 
Pol 0. 0174562925 


TRAC=-YPOS 
ROLL=PHIANG 
YAW=PSIANG 
DEPTH=-ZPOS 
DEPTH=ZPOS 
PITCH=THEANG 
BOWANG=(DB/. 01745) 
STNANG=(DS/. 01745) 


CONTINUE 
STOP 
END 


eer OBSERVER SUBROUTINE жж 
SUBROUTINE OBSER( QHADOT , THADOT , ZHADOT , QHAT , THAT , ZHAT ,DELT, 


BUS. DS,U) 


ШАНОТ--Ш  ОНАП- 0 ӨЗЕТНАТ-()2035%0)5-20;:9293%5(2Р05-2НАТ) 
ТНАрОТ-ОНАТ-14.8092%(2Р05-2НАТ) 
ZHADOT=-6*THAT+16. 45**( ZPOS -ZHAT ) 


QHAT=QHAT+DELT*QHADOT 
THAT=THAT+DELT*THADOT 
ZHAT=ZHAT+DELT* ZHADOT 
RETURN 

END 


n 


об 
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